/**
*  \file encoder_hw.c
*
*  \brief encoder driver function
*
*  \author dajin.li  <dajin.li@linde-china.cn>
*
*/

/*===============================================[ private includes  ]================================================*/
#include <xmc_gpio.h>
#include <xmc4500/io.h>
#include <xmc4500/processor.h>
#include <xmc4500/types_local.h>
#include "../sched/sched.h"
#include "../pal/pal_mem.h"
#include "../parameter/production.h"

/*===============================================[ private defines   ]================================================*/

#define QEI_CNT()                             CCU40_CC41->TIMER
#define QEI_DIR()                             (CCU40_CC41->TCST & 0x02)
#define CAPTURE_BUFFER_SIZE                   1024
#define FULL_CIRCLE                           4096

/*===============================================[ private functions ]================================================*/
static s16 get_data_location_encoder(void);
static s16 get_capture_raw_count(void);
static void update_last_counter(void);
static u32 get_capture_time(void);
static void reset_capture(void);
static void skip_encoder_pulses(void);


/*===============================================[ private variables ]================================================*/

static s32 rotor_position_raw_data;
static u8 encoder_skip = 1;
static u32 capture_time = 0;
static s16 current_counter;
static s16 last_counter;
static u32 encoder_capture_time[CAPTURE_BUFFER_SIZE];
static s8  real_dir;
static u16 encoder_steps = 64;

const u32 Float_Prescaler_TimerStep[16] = 
{
    0UL,                            /*no overflow*/
    65535UL,                        /*1  overflow*/
    196605UL,                       /*2  overflow 65535(2*2-1)*/
    458745UL,                       /*3  overflow 65535(2*2*2-1)*/ 
    983025UL,                       /*4  overflow 65535(2*2*2*2-1)*/ 
    2031585UL,                      /*5  overflow 65535(2^5-1)*/ 
    4128705UL,                      /*6  overflow 65535(2^6-1)*/ 
    8322945UL,                      /*7  overflow 65535(2^7-1)*/
    16711425UL,                     /*8  overflow 65535(2^7-1)*/
    33488385UL,                     /*9  overflow 65535(2^9-1)*/ 
    67042305UL,                     /*10 overflow 65535(2^10-1)*/
    134150145UL,                    /*11 overflow 65535(2^11-1)*/
    268365825UL,                    /*12 overflow 65535(2^12-1)*/
    536797185UL,                    /*13 overflow 65535(2^13-1)*/
    1073659905UL,                   /*14 overflow 65535(2^14-1)*/
    2147385345UL,                   /*15 overflow 65535(2^15-1)*/
};
/*===============================================[ public variables  ]================================================*/

s32 rotor_position_total;
s8 invert_speed_signal;
s32 rotor_position;
s32 motor_speed;
s32 position_timer;
s32 rotor_position_integrator = 0L;
s32 motor_speed_delta;

/*================================================[ public functions ]================================================*/

//--------------------------------------------------------------
/// \brief encoder_hardware_init
/// Initialize encoder hardware
/// \return void
//--------------------------------------------------------------

void encoder_hardware_init(void)
{
    rotor_position_raw_data = 0;
    rotor_position = 0;
    motor_speed = 0;
    rotor_position_integrator = 0;
    motor_speed_delta = 0;
    
    /* DMA Configuration for position time capture*/
    
    /*GPDMA Configuration Register*/
    /*DMA_EN    0    GPDMA Enable bit. 1B GPDMA Enabled.*/
    GPDMA0->DMACFGREG |= GPDMA0_DMACFGREG_DMA_EN_Msk;
    
    /*Source Address Register for Channel*/
    GPDMA0_CH0->SAR = (u32)&(CCU42_CC42->CV[3]);   

    /*Destination Address Register for Channel*/
    GPDMA0_CH0->DAR = (u32)encoder_capture_time;

    /*Control Register Low for Channel*/
    /*INT_EN         0       Interrupt Enable Bit*/
    /*DST_TR_WIDTH   [3:1]   Destination Transfer Width 010B 32bits*/
    /*SRC_TR_WIDTH   [6:4]   Source Transfer Width 010B 32bits*/
    /*DINC           [8:7]   Destination Address Increment 00B Increment*/
    /*SINC           [10:9]  Source Address Increment 10B No change*/
    /*DEST_MSIZE     [13:11] Destination Burst Transaction Length*/
    /*SRC_MSIZE      [16:14] Source Burst Transaction Length*/
    /*SRC_GATHER_EN  17      Source gather enable 0B Gather disabled*/
    /*DST_SCATTER_EN 18      Destination scatter enable*/
    /*TT_FC          [22:20] Transfer Type and Flow Control 010B Peripheral to Memory*/
    /*LLP_DST_EN     27      Linked List Pointer for Destination Enable*/
    /*LLP_SRC_EN     28      Linked List Pointer for Source Enable*/
    GPDMA0_CH0->CTLL = 0x00200425;
    
    /*Control Register High for Channel*/
    /*BLOCK_TS     [11:0]     Block Transfer Size , block size = 0x400*/
    GPDMA0_CH0->CTLH = 0x400;
    
    /*Configuration Register Low for Channel*/
    /*CH_PRIOR       [7:5]   Channel priority*/
    /*CH_SUSP        8       Channel Suspend*/
    /*FIFO_EMPTY     9 r     Indicates if there is data left in the channel FIFO*/
    /*HS_SEL_DST     10 rw   Destination Software or Hardware HandshakingSelect 1B Software handshaking interface.*/
    /*HS_SEL_SRC     11 rw   Source Software or Hardware Handshaking Select, 0B Hardware handshaking interface.*/
    /*LOCK_CH_L      [13:12] rw Channel Lock Level*/
    /*LOCK_B_L       [15:14] rw Bus Lock Level*/
    /*LOCK_CH        16 rw Channel Lock Bit*/
    /*LOCK_B         17 rw Bus Lock Bit*/
    /*DST_HS_POL     18 rw Destination Handshaking Interface Polarity 0B Active high*/
    /*SRC_HS_POL     19 rw Source Handshaking Interface Polarity 0B Active high*/
    /*MAX_ABRST      [29:20] rw Maximum AMBA Burst Length*/
    /*RELOAD_SRC     30 rw Automatic Source Reload*/
    /*RELOAD_DST     31 rw Automatic Destination Reload*/
    GPDMA0_CH0->CFGL = 0xC0000600;
    
    /*Configuration Register High for Channel*/
    /*FCMODE     0       rw Flow Control Mode*/
    /*FIFO_MODE  1       rw FIFO Mode Select*/
    /*PROTCTL    [4:2]   rw Protection Control*/
    /*DS_UPD_EN  5       rw Destination Status Update Enable*/
    /*SS_UPD_EN  6       rw Source Status Update Enable*/
    /*SRC_PER    [10:7]  rw Source Peripheral 02H assigns DLR line 2*/
    /*DEST_PER   [14:11] rw Destination Peripheral*/
    GPDMA0_CH0->CFGH = 0x00000104;
    
    /*Line Enable*/
    /*LNx(x = 0-7)    x  Line x Enable, enable line 2*/
    DLR->LNEN = 0x00000004;

    /*Service Request Selection 0 */
    /*RSx(x = 0-7)    [x*4+3:x*4]      rw Request Source for Line x  line 2 RS2, CCU42.SR1 = 1110B*/
    DLR->SRSEL0 = 0x00000E00;
    /*GPDMA Channel Enable Register*/
    /*CH    [7:0]     Enables/Disables the channel*/
    /*WE_CH [15:8]    Channel enable write enable*/
    GPDMA0->CHENREG = 0x101;                                   

    /* Configure POSIF0*/
    /*POSIF configuration*/
    /*FSEL      [1:0]       rw Function Selector 01B Quadrature Decoder Mode enabled*/
    /*QDCM      2           rw Position Decoder Mode selection 0B Position encoder is in Quadrature Mode*/
    /*HIDG      4           rw Idle generation enable*/
    /*MCUE      5           rw Multi-Channel Pattern SW update enable*/
    /*INSEL0    [9:8]       rw PhaseA/Hal input 1 selector 00B POSIF0.IN0A*/
    /*INSEL1    [11:10]     rw PhaseB/Hall input 2 selector 00B POSIF0.IN1A*/
    /*INSEL2    [13:12]     rw Index/Hall input 3 selector*/
    /*DSEL      16          rw Delay Pin selector*/
    /*SPES      17          rw Edge selector for the sampling trigger 0B Rising edge*/
    /*MSETS     [20:18]     rw Pattern update signal select 000B POSIF0.MSETA*/
    /*MSES      21          rw Multi-Channel pattern update trigger edge*/
    /*MSYNS     [23:22]     rw PWM synchronization signal selector*/
    /*EWIS      [25:24]     rw Wrong Hall Event selection*/
    /*EWIE      26          rw External Wrong Hall Event enable*/
    /*EWIL      27          rw External Wrong Hall Event active level*/
    /*LPC       [30:28]     rw Low Pass Filters Configuration 111B Low pass of 64 clock cycles*/
    POSIF0->PCONF = 0x70000001;

    /*Quadrature Decoder Control*/
    /*PALS      0           rw Phase A Level selector 0B Phase A is active HIGH*/
    /*PBLS      1           rw Phase B Level selector 0B Phase B is active HIGH*/
    /*PHS       2           rw Phase signals swap 0B Phase A is the leading signal for clockwise rotation*/
    /*ICM       [5:4]       rw Index Marker generations control 00B No index marker generation on POSIF0.OUT3*/
    /*DVAL      8           rh Current rotation direction 1B Clockwise rotation*/
    POSIF0->QDC = 0x100;
    /*POSIF Interrupt Enable*/
    /*ECHE      0           rw Correct Hall Event Enable*/
    /*EWHE      1           rw Wrong Hall Event Enable*/
    /*EHIE      2           rw Hall Input Update Enable*/
    /*EMST      4           rw Multi-Channel pattern shadow transfer enable*/
    /*EINDX     8           rw Quadrature Index Event Enable*/
    /*EERR      9           rw Quadrature Phase Error Enable*/
    /*ECNT      10          rw Quadrature CLK interrupt Enable*/
    /*EDIR      11          rw Quadrature direction change interrupt Enable*/
    /*EPCLK     12          rw Quadrature Period CLK interrupt Enable*/
    /*CHESEL    16          rw Correct Hall Event Service Request Selector*/
    /*WHESEL    17          rw Wrong Hall Event Service Request Selector*/
    /*HIESEL    18          rw Hall Inputs Update Event Service Request Selector*/
    /**/
    POSIF0->PFLGE = 0x800;

    /*POSIF Run Bit Set*/
    /*SRB       0           w Set Run bit*/
    POSIF0->PRUNS = 1;
    

    /* Position counter and direction triggered from POSIF0 - CCU40_CC41*/
    
    SCU_GENERAL->CCUCON |= (1 << SCU_GENERAL_CCUCON_GSC40_Pos);
    CCU40->GIDLC |= (1 << CCU4_GIDLC_CS1I_Pos);                
    CCU40->GIDLC |= (1 << CCU4_GIDLC_SPRB_Pos);  
    /*Prescaler Control*/
    CCU40_CC41->PSC = 0;                                       

    /*Slice Timer Control*/
    /*TCM     0       Timer Counting Mode 0B Edge aligned mode*/
    /*TSSM    1       Timer Single Shot Mode 0B Single shot mode is disabled*/
    /*CLST    2       Shadow Transfer on Clear*/
    /*CMOD    3       Capture Compare Mode 1B Capture Mode*/
    /*ECM     4       Extended Capture Mode 0B Normal Capture Mode.*/
    /*CAPC    [6:5]   Clear on Capture Control 11B Timer is always cleared in a capture event*/
    /*ENDM    [9:8]   Extended Stop Function Control 10B Clears the timer and run bit (flush/stop)*/
    /*STRM    10      Extended Start Function Control 1B Clears the timer and sets run bit, if not set(flush/start)*/
    /*SCE     11      Equal Capture Event enable*/
    /*CCS     12      Continuous Capture Enable*/
    /*DITHE   [14:13] Dither Enable*/
    /*DIM     15      Dither input selector*/
    /*FPE     16      Floating Prescaler enable*/
    /*TRAPE   17      rw TRAP enable 1B TRAP functionality affects the output*/
    /*TRPSE   21      TRAP Synchronization Enable*/
    /*TRPSW   22      TRAP State Clear Control*/
    /*EMS     23      External Modulation Synchronization*/
    /*EMT     24      External Modulation Type*/
    /*MCME    25      Multi Channel Mode Enable*/
    
    CCU40_CC41->TC = 0;   
    /*Timer Shadow Period Value*/
    /*PRS      [15:0]     rw Period Register*/
    CCU40_CC41->PRS = 0xff;   
    /*Timer Shadow Compare Value*/
    /*CRS      [15:0]     rw Compare Register*/
    CCU40_CC41->CRS = 0x0;
    
    /*Input Selector Configuration*/
    /*EV0IS    [3:0]     rw Event 0 signal selection 0100B CCU40.IN1E POSIF0.OUT0, PIFQD_QCLK*/
    /*EV1IS    [7:4]     rw Event 1 signal selection 0101B CCU40.IN1F POSIF0.OUT1, PIFQD_QDIR*/
    /*EV2IS    [11:8]    rw Event 2 signal selection*/
    /*EV0EM    [17:16]   rw Event 0 Edge Selection 01B Signal active on rising edge*/
    /*EV1EM    [19:18]   rw Event 1 Edge Selection 00B No action*/
    /*EV2EM    [21:20]   rw Event 2 Edge Selection*/
    /*EV0LM    22        rw Event 0 Level Selection 0B Active on HIGH level*/
    /*EV1LM    23        rw Event 1 Level Selection 0B Active on HIGH level*/
    /*EV2LM    24        rw Event 2 Level Selection*/
    /*LPF0M    [26:25]   rw Event 0 Low Pass Filter Configuration*/
    /*LPF1M    [28:27]   rw Event 1 Low Pass Filter Configuration*/
    /*LPF2M    [30:29]   rw Event 2 Low Pass Filter Configuration*/
    CCU40_CC41->INS = 0x00010054;     

    /*Connection Matrix Control*/
    /*STRTS    [1:0]    rw External Start Functionality Selector 00B External Start Function deactivated*/
    /*ENDS     [3:2]    rw External Stop Functionality Selector*/
    /*CAP0S    [5:4]    rw External Capture 0 Functionality Selector*/
    /*CAP1S    [7:6]    rw External Capture 1 Functionality Selector*/
    /*GATES    [9:8]    rw External Gate Functionality Selector*/
    /*UDS      [11:10]  rw External Up/Down Functionality Selector 10B External Up/Down Function triggered by Event 1*/
    /*LDS      [13:12]  rw External Timer Load Functionality Selector */
    /*CNTS     [15:14]  rw External Count Selector 01B External Count Function triggered by Event 0*/
    /*OFS      16       rw Override Function Selector*/
    /*TS       17       rw Trap Function Selector*/
    /*MOS      [19:18]  rw External Modulation Functionality Selector*/
    /*TCE      20       rw Timer Concatenation Enable*/    
    CCU40_CC41->CMC = 0x00004800;  

    /*Slice Timer Run Set*/
    /*TRBS     0          w Timer Run Bit set*/
    CCU40_CC41->TCSET = 1; 
    
    /*Global Channel Set*/
    CCU40->GCSS |= (1 << CCU4_GCSS_S1SE_Pos);  


    /* Speed time capture triggered of POSIF0 - CCU42_CC42 POSIF0.OUT2*/ 

    SCU_GENERAL->CCUCON |= (1 << SCU_GENERAL_CCUCON_GSC42_Pos);
    CCU42->GIDLC |= (1 << CCU4_GIDLC_CS2I_Pos);                
    CCU42->GIDLC |= (1 << CCU4_GIDLC_SPRB_Pos);                
    CCU42_CC42->PSC = 0;    
    /*Input Selector Configuration*/
    /*EV0IS    [3:0]     rw Event 0 signal selection 0100B CCU42.IN2E, POSIF0.OUT2*/
    /*EV1IS    [7:4]     rw Event 1 signal selection 0100B CCU42.IN2E, POSIF0.OUT2*/
    /*EV2IS    [11:8]    rw Event 2 signal selection*/
    /*EV0EM    [17:16]   rw Event 0 Edge Selection 01B Signal active on rising edge*/
    /*EV1EM    [19:18]   rw Event 1 Edge Selection 01B Signal active on rising edge*/
    /*EV2EM    [21:20]   rw Event 2 Edge Selection*/
    /*EV0LM    22        rw Event 0 Level Selection 0B Active on HIGH level*/
    /*EV1LM    23        rw Event 1 Level Selection*/
    /*EV2LM    24        rw Event 2 Level Selection*/
    /*LPF0M    [26:25]   rw Event 0 Low Pass Filter Configuration*/
    /*LPF1M    [28:27]   rw Event 1 Low Pass Filter Configuration*/
    /*LPF2M    [30:29]   rw Event 2 Low Pass Filter Configuration*/
    CCU42_CC42->INS = 0x00050044;   
    /*Connection Matrix Control*/
    /*STRTS    [1:0]    rw External Start Functionality Selector 00B External Start Function deactivated*/
    /*ENDS     [3:2]    rw External Stop Functionality Selector*/
    /*CAP0S    [5:4]    rw External Capture 0 Functionality Selector External Capture 0 Function triggered by Event 0*/
    /*CAP1S    [7:6]    rw External Capture 1 Functionality Selector External Capture 0 Function triggered by Event 0*/
    /*GATES    [9:8]    rw External Gate Functionality Selector*/
    /*UDS      [11:10]  rw External Up/Down Functionality Selector*/
    /*LDS      [13:12]  rw External Timer Load Functionality Selector */
    /*CNTS     [15:14]  rw External Count Selector 01B External Count Function triggered by Event 0*/
    /*OFS      16       rw Override Function Selector*/
    /*TS       17       rw Trap Function Selector*/
    /*MOS      [19:18]  rw External Modulation Functionality Selector*/
    /*TCE      20       rw Timer Concatenation Enable*/    
    CCU42_CC42->CMC = 0x00000050;   

    /*Slice Timer Control*/
    /*TCM     0       Timer Counting Mode 0B Edge aligned mode*/
    /*TSSM    1       Timer Single Shot Mode 0B Single shot mode is disabled*/
    /*CLST    2       Shadow Transfer on Clear*/
    /*CMOD    3       Capture Compare Mode 1B Capture Mode*/
    /*ECM     4       Extended Capture Mode 0B Normal Capture Mode.*/
    /*CAPC    [6:5]   Clear on Capture Control 11B Timer is always cleared in a capture event*/
    /*ENDM    [9:8]   Extended Stop Function Control 10B Clears the timer and run bit (flush/stop)*/
    /*STRM    10      Extended Start Function Control 1B Clears the timer and sets run bit, if not set(flush/start)*/
    /*SCE     11      Equal Capture Event enable*/
    /*CCS     12      Continuous Capture Enable*/
    /*DITHE   [14:13] Dither Enable*/
    /*DIM     15      Dither input selector*/
    /*FPE     16      Floating Prescaler enable*/
    /*TRAPE   17      rw TRAP enable 1B TRAP functionality affects the output*/
    /*TRPSE   21      TRAP Synchronization Enable*/
    /*TRPSW   22      TRAP State Clear Control*/
    /*EMS     23      External Modulation Synchronization*/
    /*EMT     24      External Modulation Type*/
    /*MCME    25      Multi Channel Mode Enable*/
    CCU42_CC42->TC = 0x00011068; 
    
    CCU42_CC42->PRS = 0x7f;
    
    /*Timer Shadow Compare Value*/
    /*CRS      [15:0]     rw Compare Register*/
    CCU42_CC42->CRS = 0x0;
    
    /*Service Request Selector*/
    /*POSR     [1:0]    Period/One match Service request selector*/
    /*CMSR     [3:2]    rw Compare match Service request selector*/
    /*E0SR     [9:8]    rw Event 0 Service request selector 01B Forward to CC42SR1*/
    /*E1SR     [11:10]  rw Event 1 Service request selector*/
    /*E2SR     [13:12]  rw Event 2 Service request selector*/
    CCU42_CC42->SRS = 0x00000100;
    /*E0AE     8        rw Event 0 interrupt enable*/
    /*E1AE     9        rw Event 1 interrupt enable*/
    CCU42_CC42->INTE = 0x00000300;
    CCU42_CC42->TCSET = 1;                                    
    CCU42->GCSS |= (1 << CCU4_GCSS_S2SE_Pos); 
    
    NVIC_SetPriority(GPDMA0_0_IRQn,   GPDMA0_0_INT_PRI);         
}
//--------------------------------------------------------------
/// \brief init_qei_post
/// init encoder after load eeprom
/// \return void
//--------------------------------------------------------------

void init_qei_post(void)
{
    encoder_steps = (production->encode_step)*4;
    invert_speed_signal = (production->swap_encoder);

    if(encoder_steps < 256)
    {
        encoder_steps = 256;
    }

    position_timer = 0;
    
    CCU40_CC41->PRS = encoder_steps- 1;
    CCU40->GCSS |= 0x70;                              /*Shadow transfer enable*/
    CCU40_CC41->TC |= 4;                              /*Shadow transfer on clear*/
    CCU40_CC41->TCCLR = 2;                            /*Slice timer clear*/ 
    CCU40_CC41->TC &= (~4);
}
//--------------------------------------------------------------
/// \brief calc_position_qei
/// test get rotor position
/// \return void
//--------------------------------------------------------------

void calc_position_qei(void)
{
    position_timer = QEI_CNT();
    rotor_position_raw_data = ((s32) position_timer * FULL_CIRCLE) /  encoder_steps;
    if (invert_speed_signal)
    {
        rotor_position = (- rotor_position_raw_data) & (FULL_CIRCLE - 1);
    }
    else
    {
        rotor_position = rotor_position_raw_data & (FULL_CIRCLE - 1);
    }
}
//--------------------------------------------------------------
/// \brief direction_changed
/// test direction changed or not
/// \return 1 changed, not changed
//--------------------------------------------------------------

s16 direction_changed(void)
{   
    /*Quadrature Direction Change*/
    if ((POSIF0->PFLG) & 0x00000800)
    {
        /*clear this flag*/
        POSIF0->RPFLG |= 0x00000800;    
        return(1);
    }
    else
    {   
        return(0);
    }
}
//--------------------------------------------------------------
/// \brief get_qei_dir
/// get encoder direction
/// \return direction
//--------------------------------------------------------------
s8 get_qei_dir(void)
{
    if(QEI_DIR()) /*counting down*/
    {
        return 0;
    }
    else           /*counting up*/
    {
        return 1;
    }
}
//--------------------------------------------------------------
/// \brief calc_speed_qei
/// calculate speed, units rpm, this will be called every 1ms
/// \return void
//--------------------------------------------------------------

void calc_speed_qei(void)
{
    static s32 LastPosition;
    static s32 TempSpeed;
    s32 temp1;
    static s32 lastPositionTimer;
    static s32 timer_delay = 0;

    s16 capture_count = get_capture_raw_count();
    
    if ((capture_count >= 1) && (encoder_skip))
    {
        encoder_skip = 0;
        reset_capture();
        capture_count = 0;
    }

    if (direction_changed())
    {
        skip_encoder_pulses();
        capture_count = 0;
    }

    temp1 = rotor_position_total - LastPosition;
    LastPosition = rotor_position_total;

    rotor_position_integrator += temp1;

    if(--timer_delay < 0)
    {
        motor_speed_delta = (u32)((float)rotor_position_integrator * 60.0f/(64.0f/1000.0f*4096.0f));
        rotor_position_integrator = 0;
        timer_delay = 64;
    }

    if (temp1 < 0)
    {   
        real_dir = 0;
    }
    else if (temp1 > 0)
    {   
        real_dir = -1;
    }

	if ((lastPositionTimer == position_timer) && (rotor_position_integrator == 0) && (motor_speed_delta ==0))
	{
		motor_speed = 0;
	}

    lastPositionTimer = position_timer;
    

    if ((!(encoder_skip)) && capture_count)                         
    {
        capture_time = get_capture_time();
        /*1 min = 60s/rev*/
        TempSpeed = (u32) ( 60.0f / ((float) encoder_steps * capture_time / 120000000.0f));
        if (!real_dir)
        {
            motor_speed = - TempSpeed;
        }
        else
        {
            motor_speed = TempSpeed;
        }
    }
}
//--------------------------------------------------------------
/// \brief encoder_service
/// calculate rotor position, this will be called every 1ms
/// \return void
//--------------------------------------------------------------

void encoder_service(void)
{
    static s32 old_rotor_position;
    s32 position_difference;
    s32 current_rotor_position;
    static s32 position_start_timer = 2;

    current_rotor_position = rotor_position;

    if (position_start_timer >= 0)
    {
        --position_start_timer;
        old_rotor_position = current_rotor_position;
    }

    position_difference = current_rotor_position - old_rotor_position;
    old_rotor_position = current_rotor_position;

    if (position_difference < -(FULL_CIRCLE / 2))
    {
        position_difference += FULL_CIRCLE;
    }
    else if (position_difference > (FULL_CIRCLE / 2))
    {
        position_difference -= FULL_CIRCLE;
    }

    rotor_position_total += position_difference;
}



/*===============================================[ private functions ]================================================*/

static s16 get_data_location_encoder(void)
{
    return((GPDMA0_CH0->CTLH - 1) & (CAPTURE_BUFFER_SIZE - 1));
}

static s16 get_capture_raw_count(void)
{
    s16 counter_difference;

    current_counter = get_data_location_encoder();

    counter_difference = current_counter - last_counter;

    /* Wrap around. */
    if (counter_difference < -(CAPTURE_BUFFER_SIZE / 2))
    {
        counter_difference += CAPTURE_BUFFER_SIZE;
    }
    else if (counter_difference > (CAPTURE_BUFFER_SIZE / 2))
    {
        counter_difference -= CAPTURE_BUFFER_SIZE;
    }

    return(counter_difference);
}

static void update_last_counter(void)
{
    last_counter = current_counter;
}
static void reset_capture(void)
{
    last_counter = get_data_location_encoder();
}

static void skip_encoder_pulses(void)
{
    encoder_skip = 1;
}

static u32 get_capture_time(void)
{
    u64 total_time = 0;
    s32 i;
    s32 count;
    u32 timer_cap;
    s32 current_fpcv;
    u32 new_time;

    count = MIN(get_capture_raw_count(), 32);

    if ((count != 0) && (encoder_skip == 0))
    {
        for (i = 0; i <= count - 1; i++)
        {
            timer_cap = encoder_capture_time[(current_counter - i) & (CAPTURE_BUFFER_SIZE - 1)];
            current_fpcv = (timer_cap >> 16) & 0x000f;
            new_time = ((timer_cap & 0xffff) << current_fpcv) + Float_Prescaler_TimerStep[current_fpcv];
            total_time = total_time + new_time;
        }
    }

    update_last_counter();

    return(total_time / count);
}


